#include "interval_out_io.h"

IntervalOutIo::IntervalOutIo(rclcpp::Clock::SharedPtr clock):
    clock_(clock)
{
    init();
}

IntervalOutIo::IntervalOutIo(rclcpp::Clock::SharedPtr clock, const uint32_t addr_index  ,const int on_time, const int off_time):
    clock_(clock), 
    m_cur_level(false),
    m_addr_index(addr_index), 
    m_on_time(on_time), 
    m_off_time(off_time), 
    m_start_time(clock_->now()), 
    m_stop(true),
    m_set_last_level(false)
{  
}

IntervalOutIo::~IntervalOutIo()
{
}

void IntervalOutIo::init()
{
      m_cur_level   = false;
      m_addr_index  = 0;
      m_on_time     = 0;    //ms
      m_off_time    = 0;
      m_stop        = true;
      m_set_last_level = false;
      m_start_time = clock_->now();
}

void IntervalOutIo::start()
{
    if(!m_stop){
        return ;
     }
     m_cur_level       = false;
     m_stop            = false;
     m_set_last_level  = false;
     m_start_time = clock_->now();
}

void IntervalOutIo::stop()
{
     if(m_stop){
         return ;
     }

     m_cur_level      = false;
     m_stop           = true;
     m_set_last_level = true;
     m_start_time = clock_->now();
}

//
//   The support routine
//
bool IntervalOutIo::intervalOutput(TargetLevel& set_io)
{
   
    bool  is_output = false;
    //if set stop ; return ;
    if(m_stop)
    {
        if(m_set_last_level)
        {
         m_set_last_level = false;
         is_output        = true;
         set_io.level      = 0;
         set_io.addr_index = m_addr_index;
        }
        return is_output;
    }

    //set frenquency cycle
    bool  always_on    = m_off_time == 0;
    int   intervalTime = m_cur_level ? m_on_time : m_off_time;
     
    int diff_time = (clock_->now() - m_start_time).seconds() * 1000; //ms
    if (diff_time > intervalTime)
    {
        if(!always_on)
        {
           m_cur_level = !m_cur_level;
        } else {
            m_cur_level = true;
        }
         m_start_time = clock_->now();
         is_output = true;
         set_io.level      = m_cur_level;
         set_io.addr_index = m_addr_index;
    }

    return is_output;
}

